/**********************************************************************
	Copyright (C) 2018  MisfitTech LLC,  All rights reserved.

 	MisfitTech uses a dual license model that allows the software to be used under
	a standard GPL open source license, or a commercial license.  The standard GPL
	license  requires that all software statically linked with MisfitTec Code is
	also distributed under the same GPL V2 license terms.  Details of both license
	options follow:

	- Open source licensing -
	MisfitTech is a free download and may be used, modified, evaluated and
	distributed without charge provided the user adheres to version two of the GNU
	General Public License (GPL) and does not remove the copyright notice or this
	text.  The GPL V2 text is available on the gnu.org web site

	- Commercial licensing -
	Businesses and individuals that for commercial or other reasons cannot comply
	with the terms of the GPL V2 license must obtain a low cost commercial license
	before incorporating MisfitTech code into proprietary software for distribution in
	any form.  Commercial licenses can be purchased from www.misfittech.net
	and do not require any source files to be changed.


	This code is distributed in the hope that it will be useful.  You cannot
	use MisfitTech's code unless you agree that you use the software 'as is'.
	MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
	warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
	PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
	implied, expressed, or statutory.


    Written by Trampas Stern for MisfitTech.

    Misfit Tech invests time and resources providing this open source code,
    please support MisfitTech and open-source hardware by purchasing
	products from MisfitTech, www.misifittech.net!
 *********************************************************************/
#include "commands.h"
#include "command.h"
#include "calibration.h"
#include "stepper_controller.h"
#include <stdlib.h>
#include "nonvolatile.h"
#include "Reset.h"
#include "nzs.h"
#include "ftoa.h"
#include "board.h"
#include "eeprom.h"
#include "steppin.h"

extern int32_t dataEnabled;

#define COMMANDS_PROMPT (":>")
sCmdUart UsbUart;
sCmdUart SerialUart;
sCmdUart HostUart; //uart on the step/dir pins

static int isPowerOfTwo (unsigned int x)
{
	while (((x % 2) == 0) && x > 1) /* While x is even and > 1 */
		x /= 2;
	return (x == 1);
}


CMD_STR(help,"Displays this message");
CMD_STR(getcal,"Prints the calibration table");
CMD_STR(calibrate,"Calbirates the encoder, should be done with motor disconnected from machine");
CMD_STR(testcal,"tests the calibaration of the encoder");
CMD_STR(microsteps,"gets/sets the microstep size, example 'microsteps 16'");
CMD_STR(step, "Steps motor one step, optionally direction can be set is 'step 1' for reverse");
CMD_STR(feedback, "enable or disable feedback controller, 'feedback 0' - disables, 'feedback 1' - enables");
CMD_STR(readpos, "reads the current angle as 16bit number, applies calibration if valid");
CMD_STR(encoderdiag, "Prints encoder diagnostic")
CMD_STR(spid, "with no arguments prints SIMPLE PID parameters, with arguments sets PID 'sPID Kp Ki Kd' "
		"Where Kp,Ki,Kd are floating point numbers");
CMD_STR(vpid, "with no arguments prints VELOCITY PID parameters, with arguments sets PID 'sPID Kp Ki Kd' "
		"Where Kp,Ki,Kd are floating point numbers");
CMD_STR(ppid, "with no arguments prints POSITIONAL PID parameters, with arguments sets PID 'sPID Kp Ki Kd' "
		"Where Kp,Ki,Kd are floating point numbers");
//CMD_STR(testringing ,"Steps motor at various currents and measures encoder");
//CMD_STR(microsteperror ,"test error on microstepping")
CMD_STR(dirpin, "with no arguments read dirpin setting, with argument sets direction pin rotation");
#ifndef PIN_ENABLE
CMD_STR(errorpinmode,"gets/sets the functionality of the error/enable pin");
#else
CMD_STR(enablepinmode,"gets/sets the functionality of the enable pin");
#endif

CMD_STR(errorlimit, "gets/set the error limit which will assert error pin (when error pin is set for error output)");
CMD_STR(ctrlmode, "gets/set the feedback controller mode of operation");
CMD_STR(maxcurrent, "gets/set the maximum motor current allowed in milliAmps");
CMD_STR(holdcurrent, "gets/set the motor holding current in milliAmps, only used in the simple positional PID mode");
CMD_STR(homecurrent, "gets/set the motor moving and holding currents that will be used when pin A3 is low");
CMD_STR(motorwiring, "gets/set the motor wiring direction, should only be used by experts");
CMD_STR(stepsperrotation, "gets/set the motor steps per rotation, should only be used by experts");

//CMD_STR(sysparams, "with no arguments read parameters, will set with arguments");
//CMD_STR(motorparams, "with no arguments read parameters, will set with arguments");
CMD_STR(boot, "Enters the bootloader");
CMD_STR(move, "moves encoder to absolute angle in degrees 'move 400.1'");
//CMD_STR(printdata, "prints last n error terms");
CMD_STR(velocity, "gets/set velocity in RPMs");
CMD_STR(factoryreset, "resets board to factory defaults");
CMD_STR(stop, "stops the motion planner");
CMD_STR(setzero, "set the reference angle to zero");
CMD_STR(data, "enables/disables binary data output");
CMD_STR(looptime, "returns the control loop processing time");
CMD_STR(eepromerror, "returns error in degreees from eeprom at power up realtive to current encoder");
CMD_STR(eepromloc, "returns location in degreees eeprom on power up");
CMD_STR(eepromwrite, "forces write of location to eeprom");
CMD_STR(eepromsetloc, "sets the device angle based on EEPROM last reading, compenstates for error")
CMD_STR(setpos, "sets the current angle in degrees");
CMD_STR(reboot, "reboots the unit");
CMD_STR(homepin, "sets the pin used to drop to homing current");
CMD_STR(homeangledelay, "sets the angle delay in dropping to homing current");
#ifdef PIN_ENABLE
CMD_STR(home, "moves the motor until home switch (enable pin) is pulled low. example 'home 360 0.5' move up to 360 degrees at 0.5 RPM ")
#endif
CMD_STR(pinread, "reads pins as binary (bit 0-step, bit 1 - Dir, bit 2 - Enable, bit 3 - Error, bit 4 - A3, bit 5- TX, bit 6 - RX")
CMD_STR(errorpin, "Sets the logic level of error pin")
CMD_STR(geterror, "gets current error")
CMD_STR(getsteps, "returns number of steps seen")
CMD_STR(debug, "enables debug commands out USB")
//List of supported commands
sCommand Cmds[] =
{
		COMMAND(help),
		COMMAND(calibrate),
		COMMAND(getcal),
		COMMAND(testcal),
		COMMAND(microsteps),
		COMMAND(step),
		COMMAND(feedback),
		COMMAND(readpos),
		COMMAND(encoderdiag),
		COMMAND(spid),
		COMMAND(vpid),
		COMMAND(ppid),
		//COMMAND(testringing),
		//COMMAND(microsteperror),
		COMMAND(dirpin),
#ifndef PIN_ENABLE
		COMMAND(errorpinmode),
#else
		COMMAND(enablepinmode),
#endif
		COMMAND(errorlimit),
		COMMAND(ctrlmode),
		COMMAND(maxcurrent),
		COMMAND(holdcurrent),
		COMMAND(homecurrent),
		COMMAND(motorwiring),
		COMMAND(stepsperrotation),

		//COMMAND(sysparams),
		//COMMAND(motorparams),
		COMMAND(boot),
		COMMAND(move),
		//COMMAND(printdata),
		COMMAND(velocity),
		COMMAND(factoryreset),
		COMMAND(stop),
		COMMAND(setzero),
		COMMAND(data),
		COMMAND(looptime),
		COMMAND(eepromerror),
		COMMAND(eepromloc),
		COMMAND(eepromwrite),
		COMMAND(setpos),
		COMMAND(reboot),
		COMMAND(eepromsetloc),
		COMMAND(homepin),
		COMMAND(homeangledelay),
#ifdef PIN_ENABLE
		COMMAND(home),
#endif
		COMMAND(pinread),
		COMMAND(errorpin),
		COMMAND(geterror),
		COMMAND(getsteps),
		COMMAND(debug),
		{"",0,""}, //End of list signal
};

static int debug_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	uint32_t i;
	if (argc>=1)
	{
		i=atol(argv[0]);
		SysLogDebug(i);
	}
}

static int getsteps_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	int32_t s;
	s=(int32_t)getSteps();
//	s=(int32_t)stepperCtrl.getSteps();
	CommandPrintf(ptrUart,"steps %" PRIi32 "\n\r",s);
	return 0;
}
static int geterror_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	float f;
	char str[30];
	f=ANGLE_T0_DEGREES(stepperCtrl.getLoopError());
	ftoa(f,str,2,'f');
	CommandPrintf(ptrUart,"error %s deg",str);
	return 0;
}


static int errorpin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (argc==1)
	{

		SystemParams_t params;

		memcpy(&params,&NVM->SystemParams, sizeof(SystemParams_t) );
		params.errorLogic=atol(argv[0]);

		nvmWriteSystemParms(params);
		stepperCtrl.updateParamsFromNVM();

	}
	CommandPrintf(ptrUart,"error pin assert level is %d\n\r",NVM->SystemParams.errorLogic);
	return 0;

}

static int pinread_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	uint8_t ret=0;

	if (digitalRead(PIN_STEP_INPUT))
	{
		ret |= 0x01;
	}
	if (digitalRead(PIN_DIR_INPUT))
	{
		ret |= 0x02;
	}
#ifdef PIN_ENABLE
	if (digitalRead(PIN_ENABLE))
	{
		ret |= 0x04;
	}
#endif
	if (digitalRead(PIN_ERROR))
	{
		ret |= 0x08;
	}
	if (digitalRead(PIN_A3))
	{
		ret |= 0x10;
	}
	if (digitalRead(30))
	{
		ret |= 0x20;
	}
	if (digitalRead(31))
	{
		ret |= 0x40;
	}
	CommandPrintf(ptrUart,"0x%02X\n\r",ret);
	return 0;
}

#ifdef PIN_ENABLE
static void errorPinISR(void)
{
	SmartPlanner.stop(); //stop the planner
}




static int home_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	float rpm=1;
	float startDegrees=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle());
	float finalDegrees=startDegrees+360.0;
	char str[20];
	float deg;

	if (argc>=1)
	{
		finalDegrees=startDegrees+atof(argv[0]);
	}

	if (argc>=2)
	{
		rpm=atof(argv[1]);
	}

	//setup a interrupt for the enable  pin
	attachInterrupt(digitalPinToInterrupt(PIN_ENABLE), errorPinISR, FALLING);

	SmartPlanner.moveConstantVelocity(finalDegrees,rpm);

	while(!SmartPlanner.done())
	{
		//do nothing
	}
	detachInterrupt(digitalPinToInterrupt(PIN_ENABLE));
	deg=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle());
	ftoa(deg,str,2,'f');
	CommandPrintf(ptrUart,"home is %s deg\n\r",str);
	stepperCtrl.setZero();

	return 0;
}
#endif

static int reboot_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	NVIC_SystemReset();
	return 0;
}

static int setpos_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (argc>=1)
	{
		int64_t a;
		float x;
		x=fabs(atof(argv[0]));
		a=ANGLE_FROM_DEGREES(x);
		stepperCtrl.setAngle(a);
		return 0;
	}
	return 1;
}

static int eepromwrite_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	eepromFlush();
	return 0;
}
static int eepromerror_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	Angle a;
	uint16_t error;
	float deg;
	char str[20];
	a=(Angle)PowerupEEPROM.encoderAngle;

	LOG("EEPROM encoder %d",(uint16_t)a);
	LOG("start encoder %d",(uint16_t)stepperCtrl.getStartupEncoder());
	LOG("current encoder %d",(uint16_t)stepperCtrl.getEncoderAngle());
	a=(a-(Angle)stepperCtrl.getStartupEncoder());


	deg=ANGLE_T0_DEGREES((uint16_t)a) ;
	if (deg>360.0)
	{
		deg=deg-360.0;
	}

	ftoa(deg,str,2,'f');
	CommandPrintf(ptrUart,"startup error(+/-) %s deg\n\r",str);

	a=(Angle)PowerupEEPROM.encoderAngle;
	a=(a-(Angle)stepperCtrl.getEncoderAngle());
	deg=ANGLE_T0_DEGREES((uint16_t)a);
	if (deg>360.0)
	{
		deg=deg-360.0;
	}
	ftoa(deg,str,2,'f');
	CommandPrintf(ptrUart,"current error(+/-) %s deg\n\r",str);

	return 0;
}

static int eepromsetloc_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	Angle a;
	int64_t deg;
	int32_t x;

	x=(uint32_t)PowerupEEPROM.encoderAngle-(uint32_t)stepperCtrl.getEncoderAngle();

	deg=PowerupEEPROM.angle-x;

	stepperCtrl.setAngle(deg);
	return 0;
}

static int eepromloc_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	Angle a;
	int64_t deg;
	int32_t x,y;

	deg=PowerupEEPROM.angle;

	deg=(deg*360*100)/(int32_t)ANGLE_STEPS;
	x=(deg)/100;
	y=abs(deg-(x*100));
	CommandPrintf(ptrUart,"%d.%0.2d deg\n\r",x,y);
	return 0;
}
static int looptime_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{

	CommandPrintf(ptrUart,"%dus",stepperCtrl.getLoopTime());
	return 0;
}

static int setzero_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	stepperCtrl.setZero();
	return 0;
}


static int stop_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	SmartPlanner.stop();
	return 0;
}

static int data_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);
		dataEnabled=x;
		return 0;
	}
	return 1;
}



static int stepsperrotation_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{

	if (argc == 0)
	{
		uint32_t x;
		x=NVM->motorParams.fullStepsPerRotation;
		CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x);
		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);

		if (x==200 || x==400)
		{
			MotorParams_t motorParams;

			memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );
			motorParams.fullStepsPerRotation=x;

			nvmWriteMotorParms(motorParams);
			stepperCtrl.updateParamsFromNVM();


			x=NVM->motorParams.fullStepsPerRotation;
			CommandPrintf(ptrUart,"full steps per rotation %u\n\r",x);
			CommandPrintf(ptrUart,"please power cycle board\n\r");
			return 0;
		}

	}
	CommandPrintf(ptrUart,"usage 'stepsperrotation 200' or 'stepsperrotation 400'\n\r");

	return 1;
}

static int motorwiring_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{

	if (argc == 0)
	{
		uint32_t x;
		x=NVM->motorParams.motorWiring;
		CommandPrintf(ptrUart,"motor wiring %u\n\r",x);
		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);

		if (x<=1)
		{
			MotorParams_t motorParams;

			memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );
			motorParams.motorWiring=x;

			nvmWriteMotorParms(motorParams);
			stepperCtrl.updateParamsFromNVM();


			x=NVM->motorParams.motorWiring;
			CommandPrintf(ptrUart,"motor wiring %u\n\r",x);
			CommandPrintf(ptrUart,"please power cycle board\n\r");
			return 0;
		}

	}
	CommandPrintf(ptrUart,"usage 'motorwiring 0' or 'motorwiring 1'\n\r");

	return 1;
}


static int homeangledelay_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
		float f;
		char str[30];

		if (argc == 1)
		{
			f=atof(argv[0]);

			SystemParams_t params;

			memcpy(&params,&NVM->SystemParams, sizeof(SystemParams_t) );
			params.homeAngleDelay=ANGLE_FROM_DEGREES(f);

			nvmWriteSystemParms(params);
			stepperCtrl.updateParamsFromNVM();

		}

		f=ANGLE_T0_DEGREES(NVM->SystemParams.homeAngleDelay);
		ftoa(f,str,2,'f');
		CommandPrintf(ptrUart,"home angle delay %s\n\r",str);
		return 0;
}

static int homepin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
		int32_t x;
		if (argc == 0)
		{
			x=NVM->SystemParams.homePin;
			CommandPrintf(ptrUart,"home pin %d\n\r",x);
			return 0;
		}

		if (argc == 1)
		{
			x=atol(argv[0]);

			SystemParams_t params;

			memcpy(&params,&NVM->SystemParams, sizeof(SystemParams_t) );
			params.homePin=x;

			nvmWriteSystemParms(params);
			stepperCtrl.updateParamsFromNVM();


			x=NVM->SystemParams.homePin;
			CommandPrintf(ptrUart,"home pin %d\n\r",x);
			return 0;

		}

		CommandPrintf(ptrUart, "use 'sethomepin 17' to set maximum home pin to A3");

		return 1;
}


static int homecurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	uint32_t x,y;
	if (argc == 0)
	{
		x=NVM->motorParams.homeMa;
		y=NVM->motorParams.homeHoldMa;
		CommandPrintf(ptrUart,"current %umA, %umA\n\r",x,y);
		return 0;
	}

	if (argc == 1)
	{
		x=atol(argv[0]);

		MotorParams_t motorParams;

		memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );
		motorParams.homeMa=x;

		nvmWriteMotorParms(motorParams);
		stepperCtrl.updateParamsFromNVM();


		x=NVM->motorParams.homeMa;
		y=NVM->motorParams.homeHoldMa;
		CommandPrintf(ptrUart,"current %umA, %umA\n\r",x,y);
		return 0;

	}
	if (argc == 2)
	{
		x=atol(argv[0]);
		y=atol(argv[1]);

		MotorParams_t motorParams;

		memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );
		motorParams.homeMa=x;
		motorParams.homeHoldMa=y;

		nvmWriteMotorParms(motorParams);
		stepperCtrl.updateParamsFromNVM();


		x=NVM->motorParams.homeMa;
		y=NVM->motorParams.homeHoldMa;
		CommandPrintf(ptrUart,"current %umA, %umA\n\r",x,y);
		return 0;

	}
	CommandPrintf(ptrUart, "use 'homecurrent 1000 500' to set maximum home current to 1.0A and hold to 500ma");

	return 1;
}

static int holdcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{

	if (argc == 0)
	{
		uint32_t x;
		x=NVM->motorParams.currentHoldMa;
		CommandPrintf(ptrUart,"hold current %u mA\n\r",x);
		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);

		MotorParams_t motorParams;

		memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );
		motorParams.currentHoldMa=x;

		nvmWriteMotorParms(motorParams);
		stepperCtrl.updateParamsFromNVM();


		x=NVM->motorParams.currentHoldMa;
		CommandPrintf(ptrUart,"hold current %u mA\n\r",x);
		return 0;


	}
	CommandPrintf(ptrUart, "use 'holdcurrent 1000' to set maximum current to 1.0A");

	return 1;
}


static int maxcurrent_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{

	if (argc == 0)
	{
		uint32_t x;
		x=NVM->motorParams.currentMa;
		CommandPrintf(ptrUart,"max current %u mA\n\r",x);
		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);

		MotorParams_t motorParams;

		memcpy(&motorParams,&NVM->motorParams, sizeof(motorParams) );

		motorParams.currentMa=x;
		nvmWriteMotorParms(motorParams);
		stepperCtrl.updateParamsFromNVM();


		x=NVM->motorParams.currentMa;
		CommandPrintf(ptrUart,"max current %u mA\n\r",x);
		return 0;


	}
	CommandPrintf(ptrUart, "use 'maxcurrent 2000' to set maximum current to 2.0A");

	return 1;
}



static int ctrlmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;
	if (argc == 0)
	{
		switch(NVM->SystemParams.controllerMode)
		{
		case CTRL_OFF:
			CommandPrintf(ptrUart,"controller Off(0)");
			return 0;
		case CTRL_OPEN:
			CommandPrintf(ptrUart,"controller Open-loop(1)");
			return 0;
		case CTRL_SIMPLE:
			CommandPrintf(ptrUart,"controller Simple-Position-PID(2)");
			return 0;
		case CTRL_POS_PID:
			CommandPrintf(ptrUart,"controller Current-Position-PID(3)");
			return 0;
		case CTRL_POS_VELOCITY_PID:
			CommandPrintf(ptrUart,"controller Velocity-PID(4)");
			return 0;

		}
		return 1;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=atol(argv[0]);

		if (x<=4)
		{
			SystemParams_t systemParams;

			memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

			systemParams.controllerMode=(feedbackCtrl_t)(x);

			nvmWriteSystemParms(systemParams);
			stepperCtrl.updateParamsFromNVM();

			switch(NVM->SystemParams.controllerMode)
			{
			case CTRL_OFF:
				CommandPrintf(ptrUart,"controller Off(0)");
				return 0;
			case CTRL_OPEN:
				CommandPrintf(ptrUart,"controller Open-loop(1)");
				return 0;
			case CTRL_SIMPLE:
				CommandPrintf(ptrUart,"controller Simple-Position-PID(2)");
				return 0;
			case CTRL_POS_PID:
				CommandPrintf(ptrUart,"controller Current-Position-PID(3)");
				return 0;
			case CTRL_POS_VELOCITY_PID:
				CommandPrintf(ptrUart,"controller Velocity-PID(4)");
				return 0;

			}
			return 1;
		}

	}
	CommandPrintf(ptrUart, "use 'ctrlmode [0 .. 4]' to set control mode");

	return 1;
}
static int errorlimit_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;
	char str[20];
	if (argc == 0)
	{
		float x;
		x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit);
		ftoa(x,str,2,'f');
		CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str);
		return 0;
	}

	if (argc == 1)
	{
		float x;

		x=fabs(atof(argv[0]));

		SystemParams_t systemParams;

		memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

		systemParams.errorLimit=ANGLE_FROM_DEGREES(x);

		nvmWriteSystemParms(systemParams);
		stepperCtrl.updateParamsFromNVM();

		x=ANGLE_T0_DEGREES(NVM->SystemParams.errorLimit);
		ftoa(x,str,2,'f');
		CommandPrintf(ptrUart,"errorLimit %s deg\n\r",str);
		return 0;


	}
	CommandPrintf(ptrUart, "use 'errorlimit 1.8' to set error limit to 1.8 degrees");

	return 1;
}


static int dirpin_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;

	if (argc == 0)
	{
		if (CW_ROTATION == NVM->SystemParams.dirPinRotation)
		{
			CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation);
		}else
		{
			CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation);
		}
		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=abs(atol(argv[0]));
		if (x<=1)
		{

			SystemParams_t systemParams;

			memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

			systemParams.dirPinRotation=(RotationDir_t)x;

			nvmWriteSystemParms(systemParams);
			stepperCtrl.updateParamsFromNVM();

			if (CW_ROTATION == NVM->SystemParams.dirPinRotation)
			{
				CommandPrintf(ptrUart,"dirpin CW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation);
			}else
			{
				CommandPrintf(ptrUart,"dirpin CCW(%d)\n\r",(uint32_t)NVM->SystemParams.dirPinRotation);
			}
			return 0;

		}
	}
	CommandPrintf(ptrUart, "used 'dirpin 0' for CW rotation and 'dirpin 1' for CCW");


	return 1;
}

#ifndef PIN_ENABLE
static int errorpinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;

	if (argc == 0)
	{
		if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Error pin -  Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		}else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Error pin -  Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		}else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Error pin -  Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		} else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Error pin -  Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		}

		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=abs(atol(argv[0]));
		if (x<=3)
		{

			SystemParams_t systemParams;

			memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

			systemParams.errorPinMode=(ErrorPinMode_t)x;

			nvmWriteSystemParms(systemParams);
			stepperCtrl.updateParamsFromNVM();

			if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Error pin -  Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Error pin -  Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}else if (ERROR_PIN_MODE_ERROR == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Error pin -  Error pin(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			} else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Error pin -  Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}
			return 0;

		}
	}
	CommandPrintf(ptrUart, "use 'errorpinmode 0' for enable active high, 'errorpinmode 1' for enable active low  and 'errorpinmode 2' for error output"  );


	return 1;
}
#else
static int enablepinmode_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;

	if (argc == 0)
	{
		if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Enable pin -  Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		}else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Enable pin -  Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		} else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode)
		{
			CommandPrintf(ptrUart,"Enable pin -  Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		} else
		{
			CommandPrintf(ptrUart,"UNDEFINED  Pin Mode error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
		}

		return 0;
	}

	if (argc == 1)
	{
		uint32_t x;

		x=abs(atol(argv[0]));

		if (x<=1)
		{

			SystemParams_t systemParams;

			memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

			systemParams.errorPinMode=(ErrorPinMode_t)x;

			nvmWriteSystemParms(systemParams);
			stepperCtrl.updateParamsFromNVM();

			if (ERROR_PIN_MODE_ENABLE == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Enable pin -  Enable Active High(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}else if (ERROR_PIN_MODE_ACTIVE_LOW_ENABLE == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Enable pin -  Enable active low(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}else if (ERROR_PIN_MODE_BIDIR == NVM->SystemParams.errorPinMode)
			{
				CommandPrintf(ptrUart,"Enable pin -  Bidi error(%d)\n\r",(uint32_t)NVM->SystemParams.errorPinMode);
			}
			return 0;

		}
	}
	CommandPrintf(ptrUart, "use 'enablepinmode 0' for enable active high, 'enablepinmode 1' for enable active low "  );


	return 1;
}
#endif

static int factoryreset_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	nvmErase(); //erase all of the flash
	NVIC_SystemReset();
}
static int velocity_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	int64_t x;

	if (1 == argc)
	{
		float rpm;
		rpm=atof(argv[0]);
		x=(int64_t)(DIVIDE_WITH_ROUND(rpm*ANGLE_STEPS,60)); //divide with r


		stepperCtrl.setVelocity(x);
	}
	int64_t y;
	x=(stepperCtrl.getVelocity()*100 *60)/(ANGLE_STEPS);
	y=abs(x-((x/100)*100));
	CommandPrintf(ptrUart,"Velocity is %d.%02d - %d\n\r",(int32_t)(x/100),(int32_t)y,(int32_t)stepperCtrl.getVelocity());

	return 0;
}

//
//static int printdata_cmd(sCmdUart *ptrUart,int argc, char * argv[])
//{
//	int32_t x;
//
//	stepperCtrl.printData();
//
//	return 0;
//}


static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	int32_t x,ma;
	//CommandPrintf(ptrUart, "Move %d",argc);

	if (1 == argc)
	{
		float f;

		f=atof(argv[0]);
		//		if (f>1.8)
		//			f=1.8;
		//		if (f<-1.8)
		//			f=-1.8;
		x=ANGLE_FROM_DEGREES(f);
		LOG("moving %d", x);

		stepperCtrl.moveToAbsAngle(x);
	}
	if (2 == argc)
	{
		float f,rpm,a,y;
		float pos,dx;

		f=atof(argv[0]);
		rpm=atof(argv[1]);
		//		if (f>1.8)
		//			f=1.8;
		//		if (f<-1.8)
		//			f=-1.8;

		SmartPlanner.moveConstantVelocity(f,rpm);
		return 0;
		a=360*rpm/60/1000; //rotations/100ms

		pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle());
		y=pos;
		if (y>f) a=-a;

#ifndef MECHADUINO_HARDWARE
		SerialUSB.println(f);
		SerialUSB.println(y);
		SerialUSB.println(a);
#endif

		while (abs(y-f)>(2*abs(a)))
		{
			//			SerialUSB.println();
			//			SerialUSB.println(f);
			//		SerialUSB.println(y);
			//		SerialUSB.println(a);
			y=y+a;

			x=ANGLE_FROM_DEGREES(y);
			//LOG("moving %d", x);
			stepperCtrl.moveToAbsAngle(x);
			delay(1);
			//y=stepperCtrl.getCurrentAngle();
		}
		x=ANGLE_FROM_DEGREES(f);
		LOG("moving %d", x);
		stepperCtrl.moveToAbsAngle(x);
	}

	return 0;
}

static int boot_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	initiateReset(250);
}

/*
static int microsteperror_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	int i,n,j;
	bool feedback=stepperCtrl.getFeedback();
	n=200*stepperCtrl.getMicroSteps();

	CommandPrintf(ptrUart, "Function needs fixed");
	return 0;
	stepperCtrl.feedback(false);
	for (j=0; j<2; j++)
	{
		for (i=0; i<n; i++)
		{
			int32_t e;
			stepperCtrl.requestStep(1,1);
			//stepperCtrl.step(1, 2000);
			stepperCtrl.pidFeedback();

			//average 1readings
			int32_t sum=0,ii;
			for (ii=0; ii<1; ii++)
			{
				sum+=stepperCtrl.measureError();
				stepperCtrl.pidFeedback();
			}
			e=sum/ii;
			CommandPrintf(ptrUart,"%d %d\n\r",i,e);
		}
	}
	stepperCtrl.feedback(feedback); //restore feedback
	return 0;
} */

/*
static int testringing_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	stepperCtrl.testRinging();
	return 0;
}
 */

//static int sysparams_cmd(sCmdUart *ptrUart,int argc, char * argv[])
//{
//	if (0 == argc)
//	{
//		CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps);
//		CommandPrintf(ptrUart,"dirPinRotation %d\n\r",NVM->SystemParams.dirPinRotation);
//		CommandPrintf(ptrUart,"errorLimit %d\n\r",NVM->SystemParams.errorLimit);
//		CommandPrintf(ptrUart,"errorPinMode %d\n\r",NVM->SystemParams.errorPinMode);
//		CommandPrintf(ptrUart,"controllerMode %d\n\r",NVM->SystemParams.controllerMode);
//
//	} else	if (5 == argc)
//	{
//		int32_t x;
//		SystemParams_t systemParams;
//
//		systemParams.microsteps=atol(argv[0]);
//		x=atol(argv[1]);
//		systemParams.dirPinRotation=CCW_ROTATION;
//		if (x==0)
//		{
//			systemParams.dirPinRotation=CW_ROTATION;
//		}
//		systemParams.errorLimit=atol(argv[2]);
//		systemParams.errorPinMode=(ErrorPinMode_t)atol(argv[3]);
//		systemParams.controllerMode=(feedbackCtrl_t)atol(argv[4]);
//
//		nvmWriteSystemParms(systemParams);
//		stepperCtrl.updateParamsFromNVM();
//
//		CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps);
//		CommandPrintf(ptrUart,"dirPinRotation %d\n\r",NVM->SystemParams.dirPinRotation);
//		CommandPrintf(ptrUart,"errorLimit %d\n\r",NVM->SystemParams.errorLimit);
//		CommandPrintf(ptrUart,"errorPinMode %d\n\r",NVM->SystemParams.errorPinMode);
//		CommandPrintf(ptrUart,"controllerMode %d\n\r",NVM->SystemParams.controllerMode);
//	} else
//	{
//		CommandPrintf(ptrUart, "try 'sysparams microsteps dirPinRotation errorLimit errorPinMode controllerMode'\n\r\tlike 'sysparams 16 0 327 0 2'\n\e");
//	}
//	return 0;
//}

/*
static int motorparams_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (0 == argc)
	{
		CommandPrintf(ptrUart,"currentMa %d\n\r",NVM->motorParams.currentMa);
		CommandPrintf(ptrUart,"currentHoldMa %d\n\r",NVM->motorParams.currentHoldMa);
		CommandPrintf(ptrUart,"motorWiring %d\n\r",NVM->motorParams.motorWiring);
		CommandPrintf(ptrUart,"fullStepsPerRotation %d\n\r",NVM->motorParams.fullStepsPerRotation);

	} else	if (4 == argc)
	{
		int32_t x;
		MotorParams_t motorParams;

		motorParams.currentMa=atol(argv[0]);
		motorParams.currentHoldMa=atol(argv[1]);
		motorParams.motorWiring=atol(argv[2]);
		motorParams.fullStepsPerRotation=atol(argv[3]);

		nvmWriteMotorParms(motorParams);
		stepperCtrl.updateParamsFromNVM();

		CommandPrintf(ptrUart,"currentMa %d\n\r",NVM->motorParams.currentMa);
		CommandPrintf(ptrUart,"currentHoldMa %d\n\r",NVM->motorParams.currentHoldMa);
		CommandPrintf(ptrUart,"motorWiring %d\n\r",NVM->motorParams.motorWiring);
		CommandPrintf(ptrUart,"fullStepsPerRotation %d\n\r",NVM->motorParams.fullStepsPerRotation);
	} else
	{
		CommandPrintf(ptrUart, "try 'motorparams currentMa currentHoldMa motorWiring fullStepsPerRotation'\n\r\tlike 'motroparams 2200 1500 0 200'\n\e");
	}
	return 0;
}
 */
static int vpid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	CommandPrintf(ptrUart, "args %d\n\r",argc);
	if (0 == argc)
	{
		int32_t x,y;
		x=(int32_t)NVM->vPID.Kp;
		y=abs(1000*NVM->vPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->vPID.Ki;
		y=abs(1000*NVM->vPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->vPID.Kd;
		y=abs(1000*NVM->vPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	if (3 == argc)
	{
		float Kp,Ki,Kd;
		int32_t x,y;

		Kp=atof(argv[0]);
		Ki=atof(argv[1]);
		Kd=atof(argv[2]);

		nvmWrite_vPID(Kp,Ki,Kd);
		stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters

		x=(int32_t)NVM->vPID.Kp;
		y=abs(1000*NVM->vPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->vPID.Ki;
		y=abs(1000*NVM->vPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->vPID.Kd;
		y=abs(1000*NVM->vPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	return 0;
}

static int ppid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (0 == argc)
	{
		int32_t x,y;
		x=(int32_t)NVM->pPID.Kp;
		y=abs(1000*NVM->pPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->pPID.Ki;
		y=abs(1000*NVM->pPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->pPID.Kd;
		y=abs(1000*NVM->pPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	if (3 == argc)
	{
		float Kp,Ki,Kd;
		int32_t x,y;

		Kp=atof(argv[0]);
		Ki=atof(argv[1]);
		Kd=atof(argv[2]);

		nvmWrite_pPID(Kp,Ki,Kd);
		stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters

		x=(int32_t)NVM->pPID.Kp;
		y=abs(1000*NVM->pPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->pPID.Ki;
		y=abs(1000*NVM->pPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->pPID.Kd;
		y=abs(1000*NVM->pPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	return 0;
}

static int spid_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (0 == argc)
	{
		int32_t x,y;
		x=(int32_t)NVM->sPID.Kp;
		y=abs(1000*NVM->sPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->sPID.Ki;
		y=abs(1000*NVM->sPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->sPID.Kd;
		y=abs(1000*NVM->sPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	if (3 == argc)
	{
		float Kp,Ki,Kd;
		int32_t x,y;

		Kp=atof(argv[0]);
		Ki=atof(argv[1]);
		Kd=atof(argv[2]);

		nvmWrite_sPID(Kp,Ki,Kd);
		stepperCtrl.updateParamsFromNVM(); //force the controller to use the new parameters

		x=(int32_t)NVM->sPID.Kp;
		y=abs(1000*NVM->sPID.Kp-(x*1000));
		CommandPrintf(ptrUart,"Kp %d.%03d\n\r",x,y);

		x=(int32_t)NVM->sPID.Ki;
		y=abs(1000*NVM->sPID.Ki-(x*1000));
		CommandPrintf(ptrUart,"Ki %d.%03d\n\r",x,y);

		x=(int32_t)NVM->sPID.Kd;
		y=abs(1000*NVM->sPID.Kd-(x*1000));
		CommandPrintf(ptrUart,"Kd %d.%03d\n\r",x,y);
	}
	return 0;
}
static int encoderdiag_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	char str[512];
	stepperCtrl.encoderDiagnostics(str);
	CommandPrintf(ptrUart,"%s",str);
	return 0;
}

static int readpos_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	float pos;
	int32_t x,y;

	pos=ANGLE_T0_DEGREES(stepperCtrl.getCurrentAngle());
	x=int(pos);
	y=abs((pos-x)*100);
	CommandPrintf(ptrUart,"encoder %d.%02d",x,y);
	return 0;
}
static int feedback_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (0 == argc)
	{
		CommandPrintf(ptrUart,"must pass argument, 'feedback 0' - disables, 'feedback 1' - enables");
		return 1;
	}
	stepperCtrl.feedback(atoi(argv[0]));
	return 0;
}

static int step_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	if (argc == 0 )
	{
		stepperCtrl.move(0, 1);
		//stepperCtrl.step(STEPPER_FORWARD);
	}else
	{
		int d, steps=1;
		d=atoi(argv[0]);
		if (argc >1)
		{
			steps=atoi(argv[1]);
		}
		if (1 == d)
		{
			stepperCtrl.move(1, steps);
		} else
		{
			stepperCtrl.move(0, steps);
		}
	}
	return 0;
}


static int microsteps_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	bool ret;

	if (argc != 1)
	{
		CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps);
		return 0;
	}

	int32_t x;

	x=atol(argv[0]);
	if (isPowerOfTwo(x) && x>0 && x<=256)
	{
		SystemParams_t systemParams;

		memcpy(&systemParams,&NVM->SystemParams, sizeof(systemParams) );

		systemParams.microsteps=atol(argv[0]);

		nvmWriteSystemParms(systemParams);
		stepperCtrl.updateParamsFromNVM();

		CommandPrintf(ptrUart,"microsteps %d\n\r",NVM->SystemParams.microsteps);

	}else
	{
		CommandPrintf(ptrUart,"number of microsteps must be a power of 2 between 1 and 256");
		return 1; //return error
	}

	return 0;
}


// print out the help strings for the commands
static int help_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	sCommand cmd_list;
	int i;

	//now let's parse the command
	i=0;
	memcpy(&cmd_list, &Cmds[i], sizeof(sCommand));
	while(cmd_list.function!=0)
	{

		CommandPrintf(ptrUart,(cmd_list.name));
		CommandPrintf(ptrUart,(" - "));
		CommandPrintf(ptrUart,(cmd_list.help));
		CommandPrintf(ptrUart,("\n\r"));
		i=i+1;
		memcpy(&cmd_list, &Cmds[i], sizeof(sCommand));
	}
	return 0;
}



static int getcal_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	stepperCtrl.calTable.printCalTable();
	return 0;
}

static int calibrate_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	stepperCtrl.calibrateEncoder();
	CommandPrintf(ptrUart,"Calibration DONE!\n\r");
	return 0;
}

static int testcal_cmd(sCmdUart *ptrUart,int argc, char * argv[])
{
	Angle a;
	int32_t x;

	a=stepperCtrl.maxCalibrationError();
	x=(uint16_t)a*(int32_t)360000L/(int32_t)ANGLE_MAX;

	CommandPrintf(ptrUart,"Max error is %d.%03d degrees\n\r", x/1000,abs(x)%1000);
	return 0;
}


#ifndef MECHADUINO_HARDWARE

uint8_t kbhit(void)
{
	return SerialUSB.available();
	//return SerialUSB.peek() != -1;
}
uint8_t getChar(void)
{
	return SerialUSB.read();
}
uint8_t putch(char data)
{
	return SerialUSB.write((uint8_t)data);
}

#endif
uint8_t kbhit_hw(void)
{
	return Serial5.available();
	//return SerialUSB.peek() != -1;
}
uint8_t getChar_hw(void)
{
	return Serial5.read();
}
uint8_t putch_hw(char data)
{
	return Serial5.write((uint8_t)data);
}


uint8_t kbhit_step_dir(void)
{
	return Serial1.available();
	//return SerialUSB.peek() != -1;
}
uint8_t getChar_step_dir(void)
{
	return Serial1.read();
}
uint8_t putch_step_dir(char data)
{
	return Serial1.write((uint8_t)data);
}



void commandsInit(void)
{
	
#ifndef MECHADUINO_HARDWARE
  CommandInit(&UsbUart, kbhit, getChar, putch ,NULL); //set up the UART structure
  SerialUSB.print("\n\rPower Up\n\r");
  SerialUSB.print(COMMANDS_PROMPT);
#endif
	
	CommandInit(&HostUart, kbhit_step_dir, getChar_step_dir, putch_step_dir ,NULL); //set up the UART structure for step and dir pins

#ifdef CMD_SERIAL_PORT
	CommandInit(&SerialUart, kbhit_hw, getChar_hw, putch_hw ,NULL); //set up the UART structure
	Serial5.print("\n\rPower Up\n\r");
	Serial5.print(COMMANDS_PROMPT);
#endif

}

int commandsProcess(void)
{
#ifdef USE_STEP_DIR_SERIAL
	//if the step pin is configured to the SerialCom 0 then we need to process commands
	//if PA11 (D0) is configured to perpherial C then the step pin is UART
	if (getPinMux(PIN_STEP_INPUT) ==  PORT_PMUX_PMUXE_C_Val)
	{
		//SerialUSB.println("host");
		CommandProcess(&HostUart,Cmds,' ',COMMANDS_PROMPT);
	}
#endif //USE_STEP_DIR_SERIAL


#ifdef CMD_SERIAL_PORT
	CommandProcess(&SerialUart,Cmds,' ',COMMANDS_PROMPT);
#endif

#ifndef MECHADUINO_HARDWARE
if (SerialUSB.dtr())
  {
    return CommandProcess(&UsbUart,Cmds,' ',COMMANDS_PROMPT);
  }
#endif
}
